
public class Balance extends Proportional {
	
	public Balance() {
		// initializing with 1 to prevent division with 0
		m_highestVal = 1;
		setWaitTime(5);
	}

	public void doWork() throws Exception {
		//initializing controller matrix for agressive behaviour for finding ball
		int[][] k = new int[][] {{0,1,0,1,0,1},{1,0,1,0,1,0}};
		//getting sensor values
		int s0 = getDistanceValue(0);
		int s1 = getDistanceValue(1);
		int s2 = getDistanceValue(2);
		int s3 = getDistanceValue(3);
		int s4 = getDistanceValue(4);
		int s5 = getDistanceValue(5);
		
		//getting rough values for left and right motor speed
		int aL = s0*k[0][0] + s1*k[0][1] + s2*k[0][2] + s3*k[0][3] + s4*k[0][4] + s5*k[0][5];
		int aR = s0*k[1][0] + s1*k[1][1] + s2*k[1][2] + s3*k[1][3] + s4*k[1][4] + s5*k[1][5];
		//Searching for biggest speed value
		if (aL > m_highestVal) {
			m_highestVal = aL;
		}
		if (aR > m_highestVal) {
			m_highestVal = aR;
		}
		//scaling the speed value between 0 and 5	
		int speedL = (5 * aL) / m_highestVal;
		int speedR = (5 * aR) / m_highestVal;

		this.setMotorSpeeds(speedL, speedR);
    		
	}

	public void close() throws Exception {
	}
}